Direct Trajectory Optimization of Robotic Mechanical Systems with Unscheduled Contact Sequences

In questo studio sono valutate le prestazioni dei metodi di ottimizzazione numerica come mezzi per identificare movimenti dinamici ottimi di sistemi robotici meccanici interagenti con l'ambiente tramite sequenze di contatto non programmate. Nello specifico l'attenzione è concentrata su di uno schematico modello di umanoide bidimensionale (rappresentato come una catena seriale di corpi rigidi a cinque GdL con base fissa nello spazio) impegnato nell'azione di alzarsi (sedersi) da (verso) una posizione supina o prona, entrando in contatto o distaccandosi dal terreno a seconda della necessità tramite l'utilizzo delle mani, dei gomiti, delle anche, delle ginocchia e dei piedi. Le differenti alternative nell'impostazione del problema (che comportano differenti equazioni di vincolo nel processo di ottimizzazione) sono rappresentate dall'introduzione esplicita (o meno) delle forze di contatto tra le variabili libere di ottimizzazione e dalla suddivisione della pianificazione in due successive ottimizzazioni di crescente complessità a livello dinamico. La forma dei comportamenti ottenuti e la sensitività del processo di convergenza sono valutate principalmente al variare dei parametri del modello di contatto e dei pesi associati ai termini della funzione di costo; anche alcune tecniche per guidare l'umanoide ad interagire efficacemente con l'ambiente sono discusse. Lo scopo finale di questo studio è lo sviluppo di una delle poche analisi parametriche complete sulle prestazioni raggiungibili con i metodi di ottimizzazione numerica per la pianificazione del movimento di un intero sistema dinamico con sequenze di contatto non specificate a priori. This study evaluates the performances of numerical optimization methods as a tool to identify optimal dynamic motions for robotic mechanical systems interacting with the environment through unscheduled contact sequences. Specifically the attention is focuses on a schematic two-dimensional humanoid model (represented as a fixed-base five-DoF articulated serial chain of rigid bodies) in the tasks of getting up (sitting down) from (to) supine and prone positions, opportunistically making and breaking contacts with the ground through hands, elbows, hips, knees, and feet. The different alternatives in the problem transcription (which lead to different constraint equations in the nonlinear program) are determined by the explicit introduction (or not) of contact forces among the free optimization variables and by the split of the planning into two consecutive optimizations of rising dynamic complexity. Shapes of the emergent behaviors and sensitivity of the convergence process are evaluated mainly with respect to contact model parameters and weights of cost function terms; various techniques to guide the humanoid to effectively interact with the environment are also discussed. The final aim of this study is to develop one of the very few complete parametric analysis on the performances achievable with optimization-based methods for whole-body dynamic motion planning with a priori unspecified contact sequences

A Two-Stage Trajectory Optimization Strategy for Articulated Bodies with Unscheduled Contact Sequences

In this letter, we propose a two-stage strategy for optimal control problems of robotic mechanical systems that proves to be more robust, and yet more efficient, than straightforward solution strategies. Specifically, we focus on a simplified humanoid model, represented as a two-dimensional articulated serial chain of rigid bodies, in the tasks of getting up (sitting down) from (to) the supine and prone postures. Interactions with the environment are integral parts of these motions, and a priori unscheduled contact sequences are discovered by the solver itself, opportunistically making or breaking contacts with the ground through feet, knees, hips, elbows, and hands. The present investigation analyzes the effects on the computational performance of: 1) the explicit introduction of contact forces among the optimization variables, 2) the substitution of undesired contact forces with geometric constraints that prevent interpenetrations, and 3) the splitting of the planning problem into two consecutive phases of increasing complexity. To the best of our knowledge, these tests represent the only quantitative analysis of the performances achievable with different solution strategies for optimization-based, whole-body dynamic motion planning in the presence of contacts

Warm Start of Mixed-Integer Programs for Model Predictive Control of Hybrid Systems

In hybrid Model Predictive Control (MPC), a Mixed-Integer Quadratic Program (MIQP) is solved at each sampling time to compute the optimal control action. Although these optimizations are generally very demanding, in MPC we expect consecutive problem instances to be nearly identical. This paper addresses the question of how computations performed at one time step can be reused to accelerate (warm start) the solution of subsequent MIQPs. Reoptimization is not a rare practice in integer programming: for small variations of certain problem data, the branch-and-bound algorithm allows an efficient reuse of its search tree and the dual bounds of its leaf nodes. In this paper we extend these ideas to the receding-horizon settings of MPC. The warm-start algorithm we propose copes naturally with arbitrary model errors, has a negligible computational cost, and frequently enables an a-priori pruning of most of the search space. Theoretical considerations and experimental evidence show that the proposed method tends to reduce the combinatorial complexity of the hybrid MPC problem to that of a one-step look-ahead optimization, greatly easing the online computation burden

Fast Path Planning Through Large Collections of Safe Boxes

We present a fast algorithm for the design of smooth paths (or trajectories) that are constrained to lie in a collection of axis-aligned boxes. We consider the case where the number of these safe boxes is large, and basic preprocessing of them (such as finding their intersections) can be done offline. At runtime we quickly generate a smooth path between given initial and terminal positions. Our algorithm designs trajectories that are guaranteed to be safe at all times, and it detects infeasibility whenever such a trajectory does not exist. Our algorithm is based on two subproblems that we can solve very efficiently: finding a shortest path in a weighted graph, and solving (multiple) convex optimal control problems. We demonstrate the proposed path planner on large-scale numerical examples, and we provide an efficient open-source software implementation, fastpathplanning

Shortest Paths in Graphs of Convex Sets

Given a graph, the shortest-path problem requires finding a sequence of edges with minimum cumulative length that connects a source to a target vertex. We consider a generalization of this classical problem in which the position of each vertex in the graph is a continuous decision variable, constrained to lie in a corresponding convex set. The length of an edge is then defined as a convex function of the positions of the vertices it connects. Problems of this form arise naturally in road networks, robot navigation, and even optimal control of hybrid dynamical systems. The price for such a wide applicability is the complexity of this problem, which is easily seen to be NP-hard. Our main contribution is a strong mixed-integer convex formulation based on perspective functions. This formulation has a very tight convex relaxation and allows to efficiently find globally-optimal paths in large graphs and in high-dimensional spaces

Approximate hybrid model predictive control for multi-contact push recovery in complex environments

Feedback control of robotic systems interacting with the environment through contacts is a central topic in legged robotics. One of the main challenges posed by this problem is the choice of a model sufficiently complex to capture the discontinuous nature of the dynamics but simple enough to allow online computations. Linear models have proved to be the most effective and reliable choice for smooth systems; we believe that piecewise affine (PWA) models represent their natural extension when contact phenomena occur. Discrete-time PWA systems have been deeply analyzed in the field of hybrid Model Predictive Control (MPC), but the straightforward application of MPC techniques to complex systems, such as a humanoid robot, leads to mixed-integer optimization problems which are not solvable at real-time rates. Explicit MPC methods can construct the entire control policy offline, but the resulting policy becomes too complex to compute for systems at the scale of a humanoid robot. In this paper we propose a novel algorithm which splits the computational burden between an offline sampling phase and a limited number of online convex optimizations, enabling the application of hybrid predictive controllers to higher-dimensional systems. In doing so we are willing to partially sacrifice feedback optimality, but we set stability of the system as an inviolable requirement. Simulation results of a simple planar humanoid that balances by making contact with its environment are presented to validate the proposed controller

Smooth Model Predictive Control with Applications to Statistical Learning

Statistical learning theory and high dimensional statistics have had a tremendous impact on Machine Learning theory and have impacted a variety of domains including systems and control theory. Over the past few years we have witnessed a variety of applications of such theoretical tools to help answer questions such as: how many state-action pairs are needed to learn a static control policy to a given accuracy? Recent results have shown that continuously differentiable and stabilizing control policies can be well-approximated using neural networks with hard guarantees on performance, yet often even the simplest constrained control problems are not smooth. To address this void, in this paper we study smooth approximations of linear Model Predictive Control (MPC) policies, in which hard constraints are replaced by barrier functions, a.k.a. barrier MPC. In particular, we show that barrier MPC inherits the exponential stability properties of the original non-smooth MPC policy. Using a careful analysis of the proposed barrier MPC, we show that its smoothness constant can be carefully controlled, thereby paving the way for new sample complexity results for approximating MPC policies from sampled state-action pairs.Comment: 15 pages, 1 figur

Warm Start of Mixed-Integer Programs for Model Predictive Control of Hybrid Systems

In hybrid Model Predictive Control (MPC), a Mixed-Integer Quadratic Program (MIQP) is solved at each sampling time to compute the optimal control action. Although these optimizations are generally very demanding, in MPC we expect consecutive problem instances to be nearly identical. This paper addresses the question of how computations performed at one time step can be reused to accelerate (warm start) the solution of subsequent MIQPs. Reoptimization is not a rare practice in integer programming: for small variations of certain problem data, the branch-and-bound algorithm allows an efficient reuse of its search tree and the dual bounds of its leaf nodes. In this paper we extend these ideas to the receding-horizon settings of MPC. The warm-start algorithm we propose copes naturally with arbitrary model errors, has a negligible computational cost, and frequently enables an a-priori pruning of most of the search space. Theoretical considerations and experimental evidence show that the proposed method tends to reduce the combinatorial complexity of the hybrid MPC problem to that of a one-step look-ahead optimization, greatly easing the online computation burden

Towards minimum-information adaptive controllers for robot manipulators

The aim of this paper is to move a step in the direction of determining the minimum amount of information needed to control a robot manipulator within the framework of adaptive control. Recent innovations in the state of the art show how global asymptotic trajectory tracking can be achieved despite the presence of uncertainties in the kinematic and dynamic models of the robot. However, a clear distinction between which parameters can be included among the uncertainties, and which parameters can not, has not been drawn yet. Since most of the adaptive control algorithms are built on linearly parameterized models, we propose to reformulate the problem as finding a procedure to determine whether and how a given dynamical system can be linearly parameterized with respect to a specific set of parameters. Within this framework, we show how the trajectory tracking problem of a manipulator can be accomplished with the only knowledge of the number of joints of the manipulator. As an illustrative example, we present the end-effector trajectory tracking control of a robot initialized with the kinematic model of a different robot

Approximate hybrid model predictive control for multi-contact push recovery in complex environments

Feedback control of robotic systems interacting with the environment through contacts is a central topic in legged robotics. One of the main challenges posed by this problem is the choice of a model sufficiently complex to capture the discontinuous nature of the dynamics but simple enough to allow online computations. Linear models have proved to be the most effective and reliable choice for smooth systems; we believe that piecewise affine (PWA) models represent their natural extension when contact phenomena occur. Discrete-time PWA systems have been deeply analyzed in the field of hybrid Model Predictive Control (MPC), but the straightforward application of MPC techniques to complex systems, such as a humanoid robot, leads to mixed-integer optimization problems which are not solvable at real-time rates. Explicit MPC methods can construct the entire control policy offline, but the resulting policy becomes too complex to compute for systems at the scale of a humanoid robot. In this paper we propose a novel algorithm which splits the computational burden between an offline sampling phase and a limited number of online convex optimizations, enabling the application of hybrid predictive controllers to higher-dimensional systems. In doing so we are willing to partially sacrifice feedback optimality, but we set stability of the system as an inviolable requirement. Simulation results of a simple planar humanoid that balances by making contact with its environment are presented to validate the proposed controller.Fast Multi-Contact Dynamic Planning,coordinated by M. Gabiccini, COAN CA 09.01.04.0NASA award NNX16AC49